#ifndef _ground_normal_world_
#define _ground_normal_world_

// Includes:
// --------
// Project headers
#include "config.h"
#include "depth_interval.h"

// Sys/STL
#include <iostream>
#include <string>
#include <vector>
#include <cmath>
#include <algorithm>

// ROS
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_listener.h>
#include <cv_bridge/CvBridge.h>

// PCL
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/ros/conversions.h>

// OpenCV 
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>

// Timing Macros:
// -------------
#include <sys/time.h>
#include <cstdio>
#define START()		  timeval TIMER_start; gettimeofday(&TIMER_start,NULL)
#define STOP(method)  do {\
                      timeval TIMER_res, TIMER_stop; gettimeofday(&TIMER_stop,NULL); \
                      timersub(&TIMER_stop, &TIMER_start, &TIMER_res); \
                      double TIMER_duration = (TIMER_res.tv_sec + TIMER_res.tv_usec/1000000.0); \
                      printf("%s, %12.12f\n", method, TIMER_duration); \
                      fflush(stdout);} while(0)

// Namespaces:
// ----------
using namespace std;

// -----------------------
inline bool isnan(float x)
// -----------------------
{
	// Supposedly this is how you check for NaN float values
	return x != x;
}

// -----------------------
class GeometricImageFilter
// -----------------------
{
	private:
		// --------------
		struct CameraPose
		// --------------
		{
			tf::Vector3 cameraOrigin;
			tf::Vector3 groundNormal;

			float getHeight(){ return cameraOrigin[2]; }
		};

		// ------------------
		struct FilterMessages
		// ------------------
		{
			bool valid;
			sensor_msgs::Image::ConstPtr imageMsg;
			sensor_msgs::CameraInfo::ConstPtr cameraInfoMsg;
			sensor_msgs::PointCloud2::ConstPtr stereo;
			CameraPose* cameraPose;
		};
		
		// --------
		struct VWPt
		// --------
		{
			int v;
			int w;
			VWPt(int v, int w){ this-> v = v; this->w = w; }
			bool operator<(VWPt const& p2) const { return v < p2.v || (v == p2.v && w < p2.w); }
		};

		// Publishers/Subscriptions/Transforms:
		// -----------------------------------
		ros::NodeHandle nodeHandle;
		ros::Subscriber imageSubscriber;
		ros::Subscriber cameraInfoSubscriber;
		ros::Subscriber stereoSubscriber;
		tf::TransformListener transformListener;
		sensor_msgs::CvBridge cvBridge;

		// Stored Messages & similar data:
		// ------------------------------
		sensor_msgs::Image::ConstPtr imageMsg;
		sensor_msgs::CameraInfo::ConstPtr cameraInfoMsg;
		sensor_msgs::CameraInfo::ConstPtr prevCameraInfoMsg;
		sensor_msgs::PointCloud2::ConstPtr stereo;
		CameraPose* prevCameraPose;

		// Filtering variables:
		// -------------------
		static const tf::Vector3 noTranslation;
		static const tf::Vector3 groundNormalWorld;
		vector<int> windowSizes;
		vector<float> windowScales;
		map<int, DepthInterval> dS;
		map<int, DepthInterval> dH;
		map<VWPt, DepthInterval> dC;
		vector<float> rY;

		// OpenCV:
		// ------
		cv::CascadeClassifier classifier;
		bool displayLocal;

		// ---------
		// Callbacks|
		// ---------
		void imageCallback(const sensor_msgs::Image::ConstPtr& msg);
		void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg);
		void stereoCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
		
		// ------
		// Core |
		// ------
		FilterMessages getMessages();
		CameraPose* getCameraPose(string baseFrame, string cameraFrame);
		int getMinWindowSize(const boost::array<double, 9> k, const float minAllowedSizeM, const float maxAllowedDepthM);
		vector<int> getWindowSizes(int minWindowSize, int maxWindowSize, float scaleFactor, vector<float>* windowScales = NULL);
		bool computeSizeConstraints(map<int, DepthInterval>& dS, FilterMessages& messages, const float minWorldSize, const float maxWorldSize);
		inline bool filterDepthConstraint(DepthInterval& depthConstraint, float maxDepth);
		bool filterDepthConstraints(map<int, DepthInterval>& depthConstraints, float maxDepth);
		bool computeVProjection(vector<float>& out, const boost::array<double, 9> k, int height);
		bool computeHeightConstraints(map<int, DepthInterval>& dH, vector<float>& rY, FilterMessages& messages, float minHeightM, float maxHeightM);
		bool combineConstraints(map<VWPt, DepthInterval>& dC, map<int, DepthInterval>& dS, map<int, DepthInterval>& dH, unsigned int height);
		void objectDetection(vector<cv::Rect>& objects, map<VWPt, DepthInterval>& dC, cv::Mat img, 
							 sensor_msgs::PointCloud2::ConstPtr stereo, int width, int height, int groupThreshold, double eps);

	public:
		GeometricImageFilter(const char * cascadePath, bool displayLocal = false);
		~GeometricImageFilter();
		void run();
};

#endif /* _ground_normal_world_ */
